/*

Criado por : Felipe Barreiros
Criado em  : 27 Jul 2012
Alterado em: 03 Ago 2012
Versão     : 0.2


Nome        : Firmware do Arduino Mega ADK
Arquivo     : Firmware.cpp
Descrição   : Essa é a firmware do android responsável por receber informações 
              do ângulo da pista em relação ao horizonte do robô.

Configuração: Todos as conexões devem ser PWM
              Motor Esquerdo FW   : 2
              Motor Esquerdo RW   : 3
              Motor Direito FW    : 4
              Motor Direito RW    : 5
              Habilita motores    : 6

Mensagem    : A mensagem que será enviada pelo android deverá seguir as especi-
              ficações:
           
              Posição 0: byte
                se 00: Motor Esquerdo pra frente
                       Posição 1 terá a % de velocidade
                se 01: Motor Esquerdo pra trás
                       Posição 1 terá a % de velocidade
                se 02: Motor Direito pra frente
                       Posição 1 terá a % de velocidade
                se 03: Motor Direito pra trás
                       Posição 1 terá a % de velocidade
                se 04: Desabilita Motor Esquerdo pra frente
                       Próximas posições são ignoradas
                se 05: Desabilita Motor Esquerdo pra trás
                       Próximas posições são ignoradas
                se 06: Desabilita Motor Direito pra frente
                       Próximas posições são ignoradas
                se 07: Desabilita Motor Direito pra trás
                       Próximas posições são ignoradas
                se 08: Envio do ângulo para o arduino
                       Posição 1 terá o ângulo relativo ao horizonte do robô

              Posição 1: byte
                se Posição 0 = 0, 1, 2 ou 3, porcentagem de velocidade, 0 a 255
                se Posição 0 = 4, 5, 6 ou 7, posição 1 é ignorada
                se Posição 0 = 9, ângulo relativo ao horizonte, 0 a 180

*/


// Importanto bibilitecas do Arduino
#include <Wire.h>
#include <Servo.h>

// Bibliotecas do ou para o Android
#include <Max3421e.h>
#include <Usb.h>
#include <AndroidAccessory.h>
#include </Users/Felipe/Documents/ugvbr/Arduino/ControleMotores.h>


// Definindo os pinos dos motores, LED e o pino para Habilitar os Motores
#define motorEsquerdoFW   5
#define motorEsquerdoRW   6
#define motorDireitoFW    4
#define motorDireitoRW    3
#define habilitaMotores   2

// Utilizado para o cálculo de quanto o robô virará
#define ESQUERDA 90
#define DIREITA  0

// Estado da potência de cada um dos motores
// Todos os motores sempre estão habilitados se o pino habilitaMotores
// estiver pra 255, Precisamos apenas guardar a informação da potência
// Posição 0: motorEsquerdoFW potencia {0    -   255}
// Posição 1: motorEsquerdoRW potencia {0    -   255}
// Posição 2: motorDireitoFW  potencia {0    -   255}
// Posição 3: motorDireitoRW  potencia {0    -   255}
byte potenciaMotores[4] = {0,0,0,0};

int LED = 13;

AndroidAccessory acc("FRBB",
                    "UgvBr",
                    "Projeto Ardudroid '12",
                    "1.0",
                    "http://www.android.com",
                    "0000000012345678");


// A rotina de setup é executada sempre que o reset é pressionado
void setup() {
    
    // Inicializando Serial em 115200 bps
    Serial.begin(115200);
    
    // Inicializando pinos digitais como OUTPUT
    pinMode(LED, OUTPUT);

    // Ligando o acessorio
    Serial.println("Ligando o acessorio");
    acc.powerOn();
    Serial.println("Acessorio ligado com sucesso");
    
}//fim setup)()


// O loop que é executado sempre e pra sempre.. e até mais uma vez se deixar..
void loop() {
    
    // Verifica se o acessório está conectado
    if(acc.isConnected()){
        byte msg[2];

        // Lê a mensagem enviada pelo android
        int len = acc.read(msg, sizeof(msg), 1);
        
        // Se tiver alguma coisa na mensagem, segue a documentação
        if(len > 0) {
        	Serial.print("len: ");
        	Serial.print(len);
        	Serial.print("; msg[0]: ");
        	Serial.print(msg[0]);
        	Serial.print("; msg[1]: ");
        	Serial.print(msg[1]);
        	Serial.println("");

            switch(msg[0]){
                case 0:
                case 1:
                case 2:
                case 3:
                    controlaMotor(msg[0], msg[1]);
                    break;
                case 4:
                case 5:
                case 6:
                case 7:
                    controlaMotor(msg[0], 0);
                    break;
                case 8:
                    controlaBaseadoNoAngulo(msg[1]);
                    break;

            } //fim switch(msg[0])
        }

        //delay(1000);
    }//fim if acc.isConnected()
    
}//fim loop()

// Controle de ativação dos motores
void controlaMotor(byte motor, byte potencia){

    switch(motor){
        case 0:
        case 4:
            analogWrite(motorEsquerdoFW, potencia);
            break;
        case 1:
        case 5:
            analogWrite(motorEsquerdoRW, potencia);
            break;
        case 2:
        case 6:
            analogWrite(motorDireitoFW, potencia);
            break;
        case 3:
        case 7:
            analogWrite(motorDireitoRW, potencia);
            break;
    }

    // armazena a potência do motor
    potenciaMotores[motor] = potencia;
}

// função de direcionamento do motor
void controlaBaseadoNoAngulo(byte angulo){

    // Garante que estamos trabalhando em intervalo seguro
    angulo = limiarizaAngulo(angulo);
    int direcao = 0;
    

    // Verifica se está pra esquerda ou pra direita.
    if(angulo > 90){
        direcao = ESQUERDA;
        angulo = 180 - angulo;
    }
    else 
        direcao = DIREITA;

    // Vamos considerar 9 regiões, para isso, dividí-lo por 10
    // Ficaremos com intervalos de 0 a 9
    angulo = angulo /10;

    // Vira por redução da potência dos motores por 500ms
    // Se estiver a 90°, mantém 100%
    // Se estiver a 45°, reduz motor pra 50%
    // Aqui, 9 é ótimo e 0 muito distante.
    // Nao sei se a conta está certa...
    if(direcao == ESQUERDA){
        controlaMotor(motorEsquerdoFW, potenciaMotores[0] / angulo);
        delay(500);
        controlaMotor(motorEsquerdoFW, potenciaMotores[0]);
    }
    else {
        controlaMotor(motorDireitoFW, potenciaMotores[2] / angulo);
        delay(500);
        controlaMotor(motorDireitoFW, potenciaMotores[2]);
    }
    
}//fim controlaBaseadoNoAngulo

// Garante que o byte recebido se transforme ou continue dentro de 0 a 180
// Como não pode ser menor que 0, não há correção de números negativos.
byte limiarizaAngulo(byte angulo){
    if(angulo > 0 && angulo < 180)
        return angulo;
    else if(angulo > 180)
        return 180;

}//fim limiarizaAngulo

// Vira par determinada direção por X milisegundos
// Função vira parando um dos motores por um tempo determinado
void viraParaPor(int direcao, int ms){
    //if(direcao == ESQUERDA)
    //    analogWrite(motorEsquerdoFW, )
    // desisti, não é a melhor solução
}
